#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_core/GridMap.hpp>
#include <nav_msgs/OccupancyGrid.h>
#include <grid_map_msgs/GridMap.h>
#include <grid_map_ros/GridMapRosConverter.hpp>

class ElevationToMap
{
public:
    ElevationToMap(ros::NodeHandle &nh) : nh_(nh)
    {
        ros::NodeHandle pnh_("~");
        // 读取参数
        pnh_.param<std::string>("elevation_topic", elevation_topic_, "/elevation_map");
        pnh_.param<std::string>("map_topic", map_topic_, "/local_map");
        pnh_.param<std::string>("target_frame", target_frame_, "map");
        pnh_.param<double>("map_delta_height", delta_height_, 0.3);
        pnh_.param<double>("resolution", resolution_, 0.05);
        pnh_.param<double>("width", width_, 10.0);
        pnh_.param<double>("height", height_, 10.0);

        // 订阅高程图
        sub_ = nh_.subscribe(elevation_topic_, 1, &ElevationToMap::callback, this);
        // 发布 OccupancyGrid
        pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(map_topic_, 1);

        ROS_INFO("Elevation to map node initialized with local region: %.2f x %.2f m, resolution: %.2f m/cell.", width_, height_, resolution_);
    }

private:
    void callback(const grid_map_msgs::GridMap::ConstPtr &msg)
    {
        // 转换为原始 grid_map 对象
        grid_map::GridMap originalMap;
        if (!grid_map::GridMapRosConverter::fromMessage(*msg, originalMap))
        {
            ROS_ERROR("Failed to convert GridMap message.");
            return;
        }

        // 创建新地图，设置指定分辨率和区域
        grid_map::GridMap newMap;
        newMap.setGeometry(grid_map::Length(width_, height_), resolution_, grid_map::Position(0.0, 0.0));
        newMap.setFrameId(target_frame_);
        newMap.add("elevation");

        // 插值复制原始数据到新地图
        std::vector<std::string> layers_to_copy = {"elevation"};
        if (!newMap.addDataFrom(originalMap, false, true, "", layers_to_copy))
        {
            ROS_WARN("Failed to add data from original map. This may result in incomplete data.");
        }

        // 添加 passability 层
        newMap.add("passability", 0.0);

        // 遍历新地图的每个单元格判断可通行性
        const int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
        const int dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};

        for (grid_map::GridMapIterator it(newMap); !it.isPastEnd(); ++it)
        {
            grid_map::Index idx(*it);
            if (!newMap.isValid(idx, "elevation"))
            {
                newMap.at("passability", idx) = -1.0;
                continue;
            }

            double currentElevation = newMap.at("elevation", idx);
            bool isImpassable = false;

            for (int i = 0; i < 8; ++i)
            {
                grid_map::Index neighborIdx = idx;
                neighborIdx(0) += dx[i];
                neighborIdx(1) += dy[i];

                if (newMap.isValid(neighborIdx, "elevation"))
                {
                    double diff = fabs(newMap.at("elevation", neighborIdx) - currentElevation);
                    if (diff > delta_height_)
                    {
                        isImpassable = true;
                        break;
                    }
                }
            }

            newMap.at("passability", idx) = isImpassable ? 100.0 : 0.0;
        }

        // 转换为 OccupancyGrid
        nav_msgs::OccupancyGrid occupancyGrid;
        grid_map::GridMapRosConverter::toOccupancyGrid(newMap, "passability", 0.0, 100.0, occupancyGrid);

        // 设置 header 信息
        occupancyGrid.header.frame_id = newMap.getFrameId();
        occupancyGrid.header.stamp = ros::Time::now();

        // 确保分辨率正确
        occupancyGrid.info.resolution = newMap.getResolution();
        occupancyGrid.info.width = newMap.getSize()(0);
        occupancyGrid.info.height = newMap.getSize()(1);

        pub_.publish(occupancyGrid);
    }

    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Publisher pub_;

    std::string elevation_topic_;
    std::string map_topic_;
    std::string target_frame_;
    double delta_height_;
    double resolution_;
    double width_;
    double height_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "elevation_to_map_node");
    ros::NodeHandle nh;
    ElevationToMap node(nh);
    ros::spin();
    return 0;
}